#include "scaxis.h"

SCAxis::Direction SCAxis::oppositeDir(SCAxis::Direction dir)
{
    if (dir == Positive)
    {
        return Negative;
    }
    else
    {
        return Positive;
    }
}

SCAxis::SCAxis(QString name, QObject *parent) : QObject(parent)
{
    setObjectName(name);
    MotionElementContainer::getIns()->registerItem<SCAxis>(name, this);
}

void SCAxis::needReHome()
{
    m_hasHome = false;
}

void SCAxis::init()
{
    qCDebug(motionCate(), N_FUNC);

    if (moveProtection == nullptr)
    {
        throw SilicolAbort(tr("MoveProtection is not set!"), EX_LOCATION);
    }
    homeExecutor = AxisConfig::HomeExecutorEnumInfo().nameToEnum(m_config->homeExecutor());
    if (homeExecutor == AxisConfig::Driver || homeExecutor == AxisConfig::DriverThenController)
    {
        homeDoneDi = MotionElementContainer::getIns()->getItem<SCDI>(m_config->homeDoneDiName());
        startHomeDo = MotionElementContainer::getIns()->getItem<SCDO>(m_config->startHomeDoName());
    }
    initImpl();
    if (hasAlarm())
    {
        clearAlarm();
    }
    enable();
    resetMaxVel();
    resetMaxAcc();
    m_isInit = true;
    if (!isConnectVelRatioChangedSignal)
    {
        connect(m_config, &AxisConfig::velocityRatioChanged, this, &SCAxis::scaleMaxVelImpl);
        isConnectVelRatioChangedSignal = true;
    }
}

void SCAxis::home(bool waitDone, int timeout)
{
    qCInfo(motionCate(), N_FUNC_ARG(waitDone));

    if (m_config->isBelt())
    {
        qCWarning(motionCate()) << tr("Belt ignored home cmd!");
        return;
    }

    GlobalState::getIns().waitNotPaused(name());

    m_hasHome = false;
    startHome();
    hasStop = false;
    if (waitDone)
    {
        waitHomeDone(timeout);
    }
}

void SCAxis::waitHomeDone(int timeout)
{
    if (m_config->isBelt())
    {
        qCWarning(motionCate()) << tr("Belt ignored waitHomeDone cmd!");
        return;
    }
    if (homeExecutor == AxisConfig::Driver)
    {
        waitDriverHomeDone(timeout);
    }
    else
    {
        waitControllerHomeDone(timeout);
    }
}

double SCAxis::getFeedbackPos(int cacheInvalidationTime)
{
    qint64 now = QDateTime::currentMSecsSinceEpoch();
    if (now - lastUpdateFeedbackPosTimeStamp > cacheInvalidationTime)
    {
        lastFeedbackPos = round(getFeedbackPosImpl() * 100000) / 100000;
        lastUpdateFeedbackPosTimeStamp = now;
    }
    return lastFeedbackPos;
}

void SCAxis::enable()
{
    qCDebug(motionCate(), N_FUNC);

    enableImpl();
    setCurrentPos(getFeedbackPos());
    m_isEnable = true;
}

void SCAxis::disable()
{
    qCDebug(motionCate(), N_FUNC);

    if (isRunning())
    {
        qCWarning(motionCate()) << tr("Disable %1 while running! Try to stop axis first!").arg(name());
        stop();
        QElapsedTimer timer;
        timer.start();
        while (isRunning())
        {
            if (timer.elapsed() > 300)
            {
                break;
            }
            QThread::msleep(10);
        }
    }
    disableImpl();
    m_isEnable = false;
    m_hasHome = false;
}

void SCAxis::clearAlarm()
{
    disable();
    QThread::msleep(30);
    clearErrorImpl();
    QThread::msleep(30);
    enable();
}

void SCAxis::stop()
{
    hasStop = true;
    stopImpl();
    if (homeExecutor == AxisConfig::Driver || homeExecutor == AxisConfig::DriverThenController)
    {
        startHomeDo->set(false);
    }
}

void SCAxis::stopAndResetState()
{
    stop();
    currentTargetPos = getFeedbackPos();
    resetMaxVel();
}

void SCAxis::absMoveWithLimit(double targetPos)
{
    getPosInLimitRange(targetPos);
    absMove(targetPos);
}

void SCAxis::relMoveWithLimit(double step)
{
    qCInfo(motionCate(), N_FUNC_ARG(step));

    double targetPos = getFeedbackPos() + step;
    absMoveWithLimit(targetPos);
}

void SCAxis::absMove(double targetPos, bool waitDone, double precision, int timeout)
{
    if (isAtPos(targetPos, MinStep))
    {
        currentTargetPos = targetPos;
        return;
    }

    if (isinf(targetPos) || isnan(targetPos))
    {
        throw SilicolAbort(QString("Invalid target pos: %1").arg(targetPos), EX_LOCATION);
    }

    qCInfo(motionCate(), N_FUNC_ARG(targetPos, waitDone, precision));

    GlobalState::getIns().waitNotPaused(name());

    absMoveImpl(targetPos);
    hasStop = false;
    if (waitDone)
    {
        waitArrivedPos(targetPos, precision, timeout);
    }
}

void SCAxis::relMove(double step, bool waitDone, double precision, int timeout)
{
    qCInfo(motionCate(), N_FUNC_ARG(step, waitDone, precision));

    double targetPos = getFeedbackPos() + step;
    absMove(targetPos, waitDone, precision, timeout);
}

void SCAxis::tryToMove(double targetPos, uint checkInterval, int stepCount, double precision, int timeout)
{
    double curPos = getFeedbackPos(-1);
    if (qAbs(curPos - targetPos) < MinStep)
    {
        return;
    }

    qCInfo(motionCate(), N_FUNC_ARG(targetPos, checkInterval, stepCount, precision));

    GlobalState::getIns().waitNotPaused(name());

    if (moveProtection->axisIsAbleToMove(name(), targetPos))
    {
        absMove(targetPos, true, precision);
        return;
    }

    if (stepCount < 1)
    {
        throw SilicolAbort(tr("StepCount must greater than 0!"), EX_LOCATION);
    }
    double stepSize = (targetPos - curPos) / stepCount;
    double longestPos = curPos;
    bool getNewLongestPos = false;
    ErrorHandler::tryToHandleGeneralError<void>([&] {
        QElapsedTimer timer;
        timer.start();
        while (true)
        {
            double tmpLongestPos = longestPos;
            while (true)
            {
                if (timer.elapsed() > timeout)
                {
                    throw GeneralError("Axis", tr("Try to move timeout!"));
                }
                if (qAbs(targetPos - tmpLongestPos) > qAbs(stepSize))
                {
                    tmpLongestPos += stepSize;
                }
                else
                {
                    tmpLongestPos = targetPos;
                }
                if (moveProtection->axisIsAbleToMove(name(), tmpLongestPos))
                {
                    longestPos = tmpLongestPos;
                    getNewLongestPos = true;
                    if (qFuzzyCompare(longestPos, targetPos))
                    {
                        break;
                    }
                    QThread::msleep(1);
                }
                else
                {
                    break;
                }
            }

            if (getNewLongestPos)
            {
                absForceMove(longestPos);
                getNewLongestPos = false;
            }
            if (qFuzzyCompare(longestPos, targetPos))
            {
                break;
            }
            QThread::msleep(checkInterval);
        }
    });
    waitArrivedPos(targetPos, precision);
}

void SCAxis::absForceMove(double targetPos)
{
    if (isAtPos(targetPos, MinStep))
    {
        currentTargetPos = targetPos;
        return;
    }

    if (isinf(targetPos) || isnan(targetPos))
    {
        throw SilicolAbort(QString("Invalid target pos: %1").arg(targetPos), EX_LOCATION);
    }

    qCDebug(motionCate(), N_FUNC_ARG(targetPos));

    GlobalState::getIns().waitNotPaused(name());

    if (isRunning())
    {
        ErrorHandler::tryToHandleError([&] {
            checkState();
            checkInLimitRange(targetPos);
            moveProtection->checkAxisIsAbleToMove(name(), targetPos);
            currentTargetPos = targetPos;
        });
        changeTargetPosOnlineImpl(targetPos);
        hasStop = false;
    }
    else
    {
        absMove(targetPos, false);
    }
}

void SCAxis::relForceMove(double step)
{
    qCDebug(motionCate(), N_FUNC_ARG(step));

    double targetPos = getFeedbackPos() + step;
    absForceMove(targetPos);
}

void SCAxis::relForceMoveWithLimit(double step)
{
    qCDebug(motionCate(), N_FUNC_ARG(step));

    double targetPos = getFeedbackPos() + step;
    getPosInLimitRange(targetPos);
    absForceMove(targetPos);
}

void SCAxis::scaleMaxVel(double ratio)
{
    scaleMaxVelImpl(ratio);
}

void SCAxis::scaleMaxAcc(double ratio)
{
    if (qFuzzyCompare(ratio, currentAccRatio))
    {
        return;
    }

    qCDebug(motionCate(), N_FUNC_ARG(ratio));

    if (ratio < 0.1 || ratio > 10)
    {
        qCWarning(motionCate()) << QString("Acc ratio exceeded limit! Ignored scaleMaxAcc command. Axis: %1, Ratio: "
                                           "%2, Limit: [0.1, 10].")
                                       .arg(name())
                                       .arg(ratio);
        return;
    }
    scaleMaxAccImpl(ratio);
    currentAccRatio = ratio;
}

void SCAxis::resetMaxAcc()
{
    scaleMaxAcc(1);
}

void SCAxis::resetMaxVel()
{
    scaleMaxVelImpl(m_config->velocityRatio());
}

void SCAxis::jogMove(int direction, double velocityLimit)
{
    if (direction == 0)
    {
        if (m_config->isBelt())
        {
            velocityMove(Direction::Negative);
        }
        else
        {
            moveToNegativeLimit(false, velocityLimit);
        }
    }
    if (direction == 1)
    {
        if (m_config->isBelt())
        {
            velocityMove(Direction::Positive);
        }
        else
        {
            moveToPositiveLimit(false, velocityLimit);
        }
    }
}

void SCAxis::jogMoveWithVel(int direction, double velocity)
{
    double velRatio = velocity / maxVelocity();
    jogMove(direction, velRatio);
}

void SCAxis::moveToPositiveLimit(bool waitDone, double velocityLimit)
{
    qCDebug(motionCate(), N_FUNC_ARG(waitDone, velocityLimit));

    slowMoveTo(positiveLimit(), waitDone, velocityLimit);
}

void SCAxis::moveToNegativeLimit(bool waitDone, double velocityLimit)
{
    qCDebug(motionCate(), N_FUNC_ARG(waitDone, velocityLimit));

    slowMoveTo(negativeLimit(), waitDone, velocityLimit);
}

void SCAxis::slowMoveTo(double targetPos, bool waitDone, double velocityLimit)
{
    if (velocityLimit < config()->velocityRatio())
    {
        scaleMaxVel(velocityLimit);
    }

    try
    {
        absMove(targetPos, waitDone);
        if (waitDone && velocityLimit < config()->velocityRatio())
        {
            resetMaxVel();
        }
    }
    catch (SilicoolException &se)
    {
        if (waitDone && velocityLimit < config()->velocityRatio())
        {
            resetMaxVel();
        }
        throw se;
    }
}

void SCAxis::velocityMove(SCAxis::Direction dir)
{
    if (config()->isBelt())
    {
        velocityMoveImpl(dir);
    }
    else
    {
        throw SilicolAbort(tr("Only belt can move on velocity mode!"), EX_LOCATION);
    }
}

void SCAxis::startReciprocate(double pos1, int delay1, double pos2, int delay2, int times)
{
    qCDebug(motionCate(), N_FUNC_ARG(pos1, delay1, pos2, delay2, times));

    if (isReciprocating)
    {
        qCritical() << "Reciprocating is running!";
        return;
    }
    isReciprocating = true;
    try
    {
        for (int i = 0; i < times; i++)
        {
            if (!isReciprocating)
            {
                return;
            }
            absMove(pos1);
            if (delay1 > 0)
            {
                QThread::msleep(static_cast<uint>(delay1));
            }
            if (!isReciprocating)
            {
                return;
            }
            absMove(pos2);
            if (delay2 > 0)
            {
                QThread::msleep(static_cast<uint>(delay2));
            }
        }
        isReciprocating = false;
    }
    catch (SilicoolException &se)
    {
        isReciprocating = false;
        throw se;
    }
}

void SCAxis::stopReciprocate()
{
    qCDebug(motionCate(), N_FUNC);

    isReciprocating = false;
}

void SCAxis::setMasterAxis(SCAxis *masterAxis)
{
    m_masterAxis = masterAxis;
}

SCAxis *SCAxis::masterAxis() const
{
    return m_masterAxis;
}

bool SCAxis::hasMasterAxis()
{
    return m_masterAxis != nullptr;
}

bool SCAxis::hasBindedToMaterAxis()
{
    return m_hasBindToMasterAxis;
}

bool SCAxis::bindToMasterAxis()
{
    if (!hasMasterAxis())
    {
        qCCritical(motionCate()) << tr("Bind to master axis failed! %1 did not have master axis!").arg(name());
        return false;
    }
    if (m_hasBindToMasterAxis)
    {
        return true;
    }
    else
    {
        if (bindToMasterAxisImpl(true))
        {
            m_hasBindToMasterAxis = true;
            return true;
        }
        else
        {
            return false;
        }
    }
}

bool SCAxis::unBindToMasterAxis()
{
    if (!m_hasBindToMasterAxis)
    {
        return true;
    }
    else
    {
        if (bindToMasterAxisImpl(false))
        {
            m_hasBindToMasterAxis = false;
            return true;
        }
        else
        {
            return false;
        }
    }
}

void SCAxis::softLandPosDown(QString softLandingPos, bool waitDone)
{
    auto vcmConfig = qobject_cast<VCMConfig *>(m_config);
    if (vcmConfig == nullptr)
    {
        throw SilicolAbort(
            QString("Can not cast config to VCMConfig! Config type: %1").arg(m_config->metaObject()->className()));
    }
    auto pos = vcmConfig->getPos(softLandingPos);
    softLandDown(pos->velocity(), pos->targetPos(), pos->force(), pos->margin(), waitDone, pos->timeout());
}

void SCAxis::softLandDown(double vel, double targetPos, double force, double margin, bool waitDone, int timeout)
{
    GlobalState::getIns().waitNotPaused(name());

    qCInfo(motionCate(), N_FUNC_ARG(vel, targetPos, force, margin, waitDone));
    SCTimer timer(QString("%1 softLandDown").arg(name()), motionCate(), waitDone);

    startSoftLandDown(vel, targetPos, force, margin);
    hasStop = false;
    if (waitDone)
    {
        QThread::msleep(10);
        waitSoftLandDownFinished(vel, targetPos, force, margin, timeout);
    }
}

void SCAxis::waitSoftLandDownFinished(double vel, double targetPos, double force, double margin, int timeout)
{
    ErrorHandler::tryToHandleError(
        [&] {
            QElapsedTimer timer;
            timer.start();
            while (true)
            {
                if (hasStop)
                {
                    throw ActionError(
                        "Axis",
                        QObject::tr("Axis %1 has been stopped! CurrentPos: %2").arg(name()).arg(getFeedbackPos(-1)));
                }
                QThread::msleep(10);
                if (isSoftLandDownFinished())
                {
                    return;
                }

                if (timer.elapsed() > timeout)
                {
                    stop();
                    throw ActionError("Axis", QObject::tr("Axis %1 wait soft land down finished timeout!").arg(name()));
                }
            }
        },
        [&] {
            startSoftLandDown(vel, targetPos, force, margin);
            hasStop = false;
        });
}

void SCAxis::waitSoftLandUpFinished(int timeout)
{
    ErrorHandler::tryToHandleError(
        [&] {
            QElapsedTimer timer;
            timer.start();
            while (true)
            {
                if (hasStop)
                {
                    throw ActionError(
                        "Axis",
                        QObject::tr("Axis %1 has been stopped! CurrentPos: %2").arg(name()).arg(getFeedbackPos(-1)));
                }
                QThread::msleep(10);
                if (isSoftLandUpFinished())
                {
                    return;
                }

                if (timer.elapsed() > timeout)
                {
                    stop();
                    throw ActionError("Axis", QObject::tr("Axis %1 wait soft land up finished timeout!").arg(name()));
                }
            }
        },
        [&] {
            startSoftLandUp();
            hasStop = false;
        });
}

void SCAxis::softLandUp(bool waitDone, int timeout)
{
    startSoftLandUp();
    hasStop = false;
    if (waitDone)
    {
        QThread::msleep(10);
        waitSoftLandUpFinished(timeout);
    }
}

double SCAxis::measureHeight(double vel, double force, int holdTime)
{
    double currentPos = getCurrentOutputPos();
    double targetPos = (positiveLimit() - currentPos) / 2.0 + currentPos;
    double margin = (positiveLimit() - currentPos) / 2.0 - 0.01;
    softLandDown(vel, targetPos, force, margin);
    if (holdTime > 0)
    {
        QThread::msleep(holdTime);
    }
    double height = getFeedbackPos(-1);
    softLandUp();
    return height;
}

void SCAxis::startSoftLandDown(double vel, double targetPos, double force, double margin)
{
    ErrorHandler::tryToHandleError([this, vel, targetPos, force, margin] {
        checkState();
        checkIsReady();
        checkInLimitRange(targetPos);
        softLandDownImpl(vel, targetPos, force, margin);
    });
    m_isSoftLandDown = true;
}

void SCAxis::startSoftLandUp()
{
    ErrorHandler::tryToHandleError([this] {
        checkState();
        softLandUpImpl();
    });
    m_isSoftLandDown = false;
}

void SCAxis::waitArrivedPos(double targetPos, double precision, int timeout)
{
    ErrorHandler::tryToHandleError(
        [&] {
            QElapsedTimer timer;
            timer.start();
            while (true)
            {
                if (hasStop)
                {
                    if (abs(getFeedbackPos() - targetPos) < precision)
                    {
                        return;
                    }
                    else
                    {
                        throw ActionError(
                            "Axis",
                            QObject::tr("Axis %1 has been stopped! CurrentPos: %2").arg(name()).arg(getFeedbackPos(-1)));
                    }
                }
                QThread::msleep(10);
                if (isRunning())
                {
                    continue;
                }
                double feedbackPos = getFeedbackPos();
                if (m_config->isBelt())
                {
                    if (abs(feedbackPos - targetPos) < precision || abs(feedbackPos - targetPos) - 360 < precision)
                    {
                        return;
                    }
                }
                else
                {
                    if (abs(feedbackPos - targetPos) < precision)
                    {
                        return;
                    }
                }
                if (timer.elapsed() > timeout)
                {
                    stop();
                    throw ActionError("Axis", QObject::tr("Axis %1 wait arrived target pos %2 with inpos precision %3 "
                                                          "timeout! CurrentPos: %4")
                                                  .arg(name())
                                                  .arg(targetPos)
                                                  .arg(precision)
                                                  .arg(getFeedbackPos(-1)));
                }
            }
        },
        [&] {
            absMoveImpl(targetPos);
            hasStop = false;
        });
}

void SCAxis::waitArrivedPos(double precision, int timeout)
{
    waitArrivedPos(currentTargetPos, precision, timeout);
}

void SCAxis::waitGreaterThanPos(double targetPos, double precision, int timeout)
{
    ErrorHandler::tryToHandleError([&] {
        QElapsedTimer timer;
        timer.start();
        while (true)
        {
            if (getFeedbackPos() > (targetPos - precision))
            {
                return;
            }
            if (hasStop)
            {
                throw SilicolAbort(QObject::tr("Axis %1 has been stopped! Wait greater than pos failed! Target pos: "
                                               "%2, Current pos: %3, inpos precision: %4")
                                       .arg(name())
                                       .arg(targetPos)
                                       .arg(getFeedbackPos(-1))
                                       .arg(precision));
            }

            QThread::msleep(10);
            if (timer.elapsed() > timeout)
            {
                throw ActionError("Axis", QObject::tr("Axis %1 wait greater than pos timeout! Target pos:%2, Current "
                                                      "pos: %3, inpos precision %4")
                                              .arg(name())
                                              .arg(targetPos)
                                              .arg(getFeedbackPos(-1))
                                              .arg(precision));
            }
        }
    });
}

void SCAxis::waitLessThanPos(double targetPos, double precision, int timeout)
{
    ErrorHandler::tryToHandleError([&] {
        QElapsedTimer timer;
        timer.start();
        while (true)
        {
            if (getFeedbackPos() < (targetPos + precision))
            {
                return;
            }
            if (hasStop)
            {
                throw SilicolAbort(QObject::tr("Axis %1 has been stopped! Wait less than pos failed! Target pos: "
                                               "%2, Current pos: %3, inpos precision: %4")
                                       .arg(name())
                                       .arg(targetPos)
                                       .arg(getFeedbackPos(-1))
                                       .arg(precision));
            }

            QThread::msleep(10);
            if (timer.elapsed() > timeout)
            {
                throw ActionError("Axis", QObject::tr("Axis %1 wait less than pos timeout! Target pos:%2, Current "
                                                      "pos: %3, inpos precision %4")
                                              .arg(name())
                                              .arg(targetPos)
                                              .arg(getFeedbackPos(-1))
                                              .arg(precision));
            }
        }
    });
}

void SCAxis::checkState(bool checkHasHome)
{
    if (!isInit())
    {
        throw ActionError("Axis", QObject::tr("Axis %1 has not been inited!").arg(name()));
    }
    if (!isEnable())
    {
        throw ActionError("Axis", QObject::tr("Axis %1 has not been enabled!").arg(name()));
    }
    if (checkHasHome && !m_config->isBelt() && !hasHome())
    {
        throw ActionError("Axis", QObject::tr("Axis %1 has not been homed!").arg(name()));
    }
    if (hasAlarm())
    {
        throw ActionError("Axis", QObject::tr("Axis %1 has Alarm!").arg(name()), "Please clear alarm first!");
    }
}

void SCAxis::checkIsReady()
{
    if (isRunning())
    {
        bool gotIt = false;
        QElapsedTimer timer;
        timer.start();
        while (timer.elapsed() < 80)
        {
            if (!isRunning())
            {
                gotIt = true;
                break;
            }
            QThread::msleep(10);
        }
        if (!gotIt)
        {
            throw ActionError("Axis", QObject::tr("Axis %1 is running!").arg(name()));
        }
    }
    if (!isReady())
    {
        bool gotIt = false;
        QElapsedTimer timer;
        timer.start();
        while (timer.elapsed() < 500)
        {
            if (isReady())
            {
                gotIt = true;
                break;
            }
            QThread::msleep(10);
        }
        if (!gotIt)
        {
            throw ActionError("Axis", QObject::tr("Axis %1 is not ready!").arg(name()));
        }
    }
}

void SCAxis::checkInLimitRange(double targetPos) const
{
    if ((targetPos > positiveLimit() && targetPos > negativeLimit())
        || (targetPos < positiveLimit() && targetPos < negativeLimit()))
    {
        throw ActionError("Axis", QObject::tr("Axis %1 target pos %2 exceeded limit [%3, %4]!")
                                      .arg(name())
                                      .arg(targetPos)
                                      .arg(negativeLimit())
                                      .arg(positiveLimit()));
    }
}

void SCAxis::startHome()
{
    ErrorHandler::tryToHandleError([&] {
        checkState(false);
        checkIsReady();
        if (homeExecutor == AxisConfig::Driver || homeExecutor == AxisConfig::DriverThenController)
        {
            qCInfo(motionCate()) << tr("%1 start home. Handled by driver.").arg(name());
            if (startHomeDo->get())
            {
                startHomeDo->set(false);
                QThread::msleep(100);
            }
            startHomeDo->set(true);
            if (homeExecutor == AxisConfig::DriverThenController)
            {
                hasStop = false;
                waitDriverHomeDone();
                QThread::msleep(100);
                relMove(m_config->homeOffsetBetweenDriverAndController());
                m_hasHome = false;
                homeImpl();
            }
        }
        else
        {
            homeImpl();
        }
    });
}

void SCAxis::absMoveImpl(double targetPos)
{
    ErrorHandler::tryToHandleError([&] {
        checkState();
        checkIsReady();
        checkInLimitRange(targetPos);
        moveProtection->checkAxisIsAbleToMove(name(), targetPos);
        currentTargetPos = targetPos;
        moveToImpl(targetPos);
    });
}

void SCAxis::getPosInLimitRange(double &targetPos)
{
    if (targetPos > positiveLimit())
    {
        targetPos = positiveLimit();
    }
    else if (targetPos < negativeLimit())
    {
        targetPos = negativeLimit();
    }
}

void SCAxis::waitControllerHomeDone(int timeout)
{
    ErrorHandler::tryToHandleError(
        [&] {
            QElapsedTimer timer;
            timer.start();
            while (true)
            {
                if (hasStop)
                {
                    throw ActionError("Axis", QObject::tr("Axis %1 has been stopped!").arg(name()));
                }
                if (isHomeDone())
                {
                    QString homeErrMsg = homeErrorMsg();
                    if (!homeErrMsg.isEmpty())
                    {
                        throw ActionError("Axis",
                                          QObject::tr("Axis %1 home failed!\r\n%2").arg(name()).arg(homeErrMsg));
                    }
                    if (hasStop)
                    {
                        throw ActionError("Axis", QObject::tr("Axis %1 has been stopped!").arg(name()));
                    }
                    operationAfterHome();
                    m_hasHome = true;
                    qCDebug(motionCate()) << tr("Axis %1 home done!").arg(name());
                    return;
                }
                QThread::msleep(10);
                if (timer.elapsed() > timeout)
                {
                    stopHome();
                    throw ActionError("Axis", QObject::tr("Axis %1 wait home done timeout!").arg(name()));
                }
            }
        },
        [&] {
            startHome();
            hasStop = false;
        });
}

void SCAxis::waitDriverHomeDone(int timeout)
{
    QThread::msleep(50);
    ErrorHandler::tryToHandleError(
        [&] {
            QElapsedTimer timer;
            timer.start();
            while (true)
            {
                if (hasStop)
                {
                    throw ActionError("Axis", QObject::tr("Axis %1 has been stopped!").arg(name()));
                }
                if (homeDoneDi->get())
                {
                    operationAfterHome();
                    startHomeDo->set(false);
                    m_hasHome = true;
                    qCDebug(motionCate()) << tr("Axis %1 home done!").arg(name());
                    return;
                }
                QThread::msleep(10);
                if (timer.elapsed() > timeout)
                {
                    startHomeDo->set(false);
                    throw ActionError("Axis", QObject::tr("Axis %1 wait home done timeout!").arg(name()));
                }
            }
        },
        [&] {
            startHome();
            hasStop = false;
        });
}
